#include "rtthread.h"
#include "rtdevice.h"

#include "pid.h"
#include "srv_sensor.h"
#include "drv_motor.h"
#include "task_fun.h"
#include "master.h"

extern PID_Data turn_pid;

static uint8_t road_signal_num=0;
static uint8_t origin_signal_count=0;

static rt_device_t serial_k210;  
static rt_device_t serial_talk;

rt_sem_t k210_rx_sem;
static uint8_t k210_re_finsh_flag=1;


void ist_turn_low(uint16_t delay_time_ms, uint8_t low_level)
{
    // ist_power_down(low_level);
     ist_pid_setmax(&turn_pid, 8);

    ist_pid_fix(&turn_pid, K_TYPE_P, TURN_KP/2);

    rt_thread_mdelay(delay_time_ms);

    // ist_power_up();
     ist_pid_setmax(&turn_pid, 100);

    ist_pid_fix(&turn_pid, K_TYPE_P, TURN_KP);
}

void ist_signal_stop_5s(void)
{
    ist_motor_set(MOTOR_LEFT, 0.0);
    ist_motor_set(MOTOR_RIGHT, 0.0);
    // rt_thread_mdelay(5000);
}

void ist_origin_signal_add(void)
{
    origin_signal_count++;
}

uint8_t ist_get_origin_signal_count(void)
{
    return origin_signal_count;
}

static void _k210_uart_input(rt_device_t dev, rt_size_t size)
{
    // if(k210_re_finsh_flag)
    {
        char re;
        rt_device_read(dev, 0, &re, 1);
        // rt_kprintf("re:%c\n",re);
        switch (re)
        {
            case 'f':
                road_signal_num = SIGNAL_FORK;
                break;

            case 's':
                road_signal_num = SIGNAL_STOP;
                break;

            case 'o':
                road_signal_num = SIGNAL_ORIGIN;
                break;
            
            default:
                road_signal_num = 0;
                break;
            }
    }
    rt_sem_release(k210_rx_sem);
}

void  ist_car_talk(uint8_t cmd)
{
    rt_device_write(serial_talk, 0, &cmd, 1);
    rt_device_write(serial_talk, 0, "\n", 1);
}

// void ist_k210_re_finish(void)
// {
//     k210_re_finsh_flag = 1 ;
// }

uint8_t ist_get_road_signal_num(void)
{
    return road_signal_num;
}

void ist_clean_road_signal_num(void)
{
    road_signal_num = 0;
}



static void ist_two_uart_init(void)
{
    struct serial_configure config_1 = RT_SERIAL_CONFIG_DEFAULT;  /* 初始化配置参数 */
    config_1.bufsz  = 2;

    serial_k210 = rt_device_find(K210_UART);
    rt_device_control(serial_k210, RT_DEVICE_CTRL_CONFIG, &config_1);
    rt_device_open(serial_k210, RT_DEVICE_FLAG_INT_RX);
    rt_device_set_rx_indicate(serial_k210, _k210_uart_input);

    k210_rx_sem = rt_sem_create("k210_rx_sem", 0, RT_IPC_FLAG_FIFO);

    struct serial_configure config_2 = RT_SERIAL_CONFIG_DEFAULT;  /* 初始化配置参数 */
    config_2.baud_rate  = 9600;

    serial_talk = rt_device_find(TALK_UART);
    rt_device_control(serial_talk, RT_DEVICE_CTRL_CONFIG, &config_2);
    if(rt_device_open(serial_talk, RT_DEVICE_FLAG_INT_RX) == RT_EOK)
    {
        rt_kprintf("serial_talk  ok\n");
    }
    else
    {
        rt_kprintf("serial_talk  fail\n");
    }

    char test[] = "this is a test\n";
    rt_device_write(serial_talk, 0, &test, 10);
}

INIT_COMPONENT_EXPORT(ist_two_uart_init);



// static void test(void)
// {
//     while(1)
//     {
//         rt_sem_take(k210_rx_sem, RT_WAITING_FOREVER);
//         rt_kprintf("k210_re:%d\n",road_signal_num);
//         // rt_device_write(serial_talk,0,&road_signal_num,1);
//     }
// }

// void ist_test_start(void)
// {
//     // ist_two_uart_init();
//     rt_thread_t thread = rt_thread_create("test", test, RT_NULL, 1024, 2, 10);
//     rt_thread_startup(thread);

// }

// INIT_COMPONENT_EXPORT(ist_test_start);

